#include "giftWrapping.h"
#include "face.h"
#include "edge.h"
#include "jarvis.h"
#include <algorithm>
#include <cmath>

#define ERROR 0.00001
#define ERROR2 0.1

GiftWrapping::GiftWrapping() : Methods(){
}

GiftWrapping::~GiftWrapping(){
}

void GiftWrapping::calculate(Object *_obj){
    if(_obj->sizeVertices() == 3){
        std::cout <<"3 pontos" << std::endl;
        Jarvis jarvis;
        std::vector<Vertex *> vertices = _obj->getVertices();
        Vertex *back = vertices.back();
        vertices.pop_back();
        Point vec = vertices.front()->getPoint() - back->getPoint();
        jarvis.calculate(_obj, vertices, vec, back);
        return;
    }

    Face *face, *newFace;
    Edge *edge;
    bool edgeSignal;


    std::vector<Face *> facesList;              //Lista de faces, F, a serem visitadas
    face = firstFace(_obj);
    facesList.push_back(face);       //Incluindo a primeira face em F

    while(!facesList.empty()){                  //Enquanto F != vazia
        face = facesList.front();
        facesList.erase(facesList.begin());

        edge = face->getEdge();
        edgeSignal = face->getEdgeSignal();
        do{

            if (!(edge->getClockwise() && edge->getCounterClockwise())){
                newFace = findNextFace(_obj, face, edge, edgeSignal);
                facesList.push_back(newFace);
                newFace->setIsBoundary(true);
//                if (newFace->getID() == 1) return;
            }

            if(edgeSignal){
                edgeSignal = edge->getNextClockwiseSignal();
                edge = edge->getNextClockwise();
            }else{
                edgeSignal = edge->getNextCounterClockwiseSignal();
                edge = edge->getNextCounterClockwise();
            }
        }while (edge != face->getEdge());
    }



    std::cout << "EMBLULHO FEITO!"<<std::endl;
    _obj->print();
    //LEMBRAR DE SMEPRE ADICIONAR UMA ARESTA DEPOIS SETAR SEUS PARAMETROS!
}

Face *GiftWrapping::firstFace(Object *_obj){
    //achar o vertice no menor Z
    std::vector<Vertex *> originalVertices = _obj->getVertices();
    sort(originalVertices.begin(), originalVertices.end(), lessThanByZ);

    //criar dois pontos auxiliares
    Vertex *v1 = originalVertices.front();
    v1->setIsBoundary(true);
    Vertex vAuxX = Vertex(-originalVertices.size() - 1, (v1->getPoint() + Point(1, 0, 0)));
    Vertex vAuxY = Vertex(-originalVertices.size() - 2, (v1->getPoint() + Point(0, 1, 0)));

    Edge eAux1 = Edge(-2, v1, &vAuxY);
    Edge eAux2 = Edge(-3, &vAuxY, &vAuxX);
    Edge eAux3 = Edge(-4, &vAuxX, v1);

    Face fAux = Face(-2, NULL, &eAux1, true, true);

    v1->setEdge(&eAux1, true);
    vAuxY.setEdge(&eAux2, true);
    vAuxX.setEdge(&eAux3, true);

    eAux1.setClockwise(&fAux);
    eAux2.setClockwise(&fAux);
    eAux3.setClockwise(&fAux);

    eAux1.setNextClockwise(&eAux2, true);
    eAux2.setNextClockwise(&eAux3, true);
    eAux3.setNextClockwise(&eAux1, true);

    eAux1.setPreviousClockwise(&eAux3, true);
    eAux2.setPreviousClockwise(&eAux1, true);
    eAux3.setPreviousClockwise(&eAux2, true);

    std::vector<Vertex *> vertices = findNextVertex(_obj, &fAux, &eAux1, true);

    v1->setEdge(NULL, true);


    //achar o segundo e o terceiro pontos para fazer a face inicial
    //caso ache mais de um, fazer o jarvis
    if(vertices.size() > 1){
        //adiciona a nova face, vertices e as arestas criadas
        Jarvis jarvis;
        Face *f = jarvis.calculate(_obj, vertices, firstAxis(vertices, v1, originalVertices), v1);

        vertices.push_back(v1);
        f->setVerticesInThePlan(vertices);
        return f;
    }

    //achado o 2º ponto
    Vertex *v2 = vertices.front();
    v2->setIsBoundary(true);

    Edge *e1 = new Edge(edgeID++, v2, v1, true);   //não é necessario chegar pq é a primeira vez que se cria uma aresta
    _obj->addEdge(&e1);
    eAux2 = Edge(-3, v2, &vAuxY);

    fAux = Face(-2, NULL, e1, false, true);

    v1->setEdge(&eAux1, true);
    v2->setEdge(e1, true);
    vAuxY.setEdge(&eAux2, false);

    e1->setCounterClockwise(&fAux);
    eAux2.setClockwise(&fAux);
    eAux1.setCounterClockwise(&fAux);
    eAux1.setClockwise(NULL);

    e1->setNextCounterClockwise(&eAux2, true);
    eAux2.setNextClockwise(&eAux1, false);
    eAux1.setNextCounterClockwise(e1, false);
    eAux1.setNextClockwise(NULL, true);

    e1->setPreviousCounterClockwise(&eAux1, false);
    eAux2.setPreviousClockwise(e1, false);
    eAux1.setPreviousCounterClockwise(&eAux2, true);
    eAux1.setPreviousClockwise(NULL, true);

    vertices = findNextVertex(_obj, &fAux, e1, false);

    //independente de quantos vertices, tem que apagar as auxiliares
    e1->setCounterClockwise(NULL);
    e1->setNextCounterClockwise(NULL, true);
    e1->setPreviousCounterClockwise(NULL, true);
    v1->setEdge(NULL, true);


    //caso ache mais de um, fazer o jarvis
    if(vertices.size() > 1){
        //adiciona a nova face, vertices e as arestas criadas
        vertices.push_back(v1);
        Jarvis jarvis;
        Face *f = jarvis.calculate(_obj, vertices, v1->getPoint() - v2->getPoint(), v2);

        vertices.push_back(v1);
        vertices.push_back(v2);
        f->setVerticesInThePlan(vertices);
        return f;
    }

    //setando a primeira face
    Vertex *v3 = vertices.front();
    v3->setIsBoundary(true);

    Edge *e2 = new Edge(edgeID++, v1, v3, true);   //não é necessario chegar pq é a segunda vez que se cria uma aresta
    Edge *e3 = new Edge(edgeID++, v3, v2, true);   //não é necessario chegar pq é a terceira vez que se cria uma aresta... assim tendo a primeira face
    _obj->addEdge(&e2);
    _obj->addEdge(&e3);

    Face *f = new Face(faceID++, _obj, e1, true, true);
    _obj->addFace(f);

    v1->setEdge(e2, true);
    v3->setEdge(e3, true);

    e1->setClockwise(f);
    e2->setClockwise(f);
    e3->setClockwise(f);

    e1->setNextClockwise(e2, true);
    e2->setNextClockwise(e3, true);
    e3->setNextClockwise(e1, true);

    e1->setPreviousClockwise(e3, true);
    e2->setPreviousClockwise(e1, true);
    e3->setPreviousClockwise(e2, true);

    vertices.push_back(v1);
    vertices.push_back(v2);
    f->setVerticesInThePlan(vertices);

    return f;
}

Face *GiftWrapping::findNextFace(Object *_obj, Face *_f, Edge *_e, bool _es){
    std::vector<Vertex *> vertices = findNextVertex(_obj, _f, _e, _es);

    //caso ache mais de um, fazer o jarvis
    if(vertices.size() > 1){
        //adiciona a nova face, vertices e as arestas criadas
        Jarvis jarvis;
        Face *f;
        if(_es){
            vertices.push_back(_e->getEnd());
            f = jarvis.calculate(_obj, vertices, _e->getStart()->getPoint() - _e->getEnd()->getPoint(), _e->getStart());

            vertices.push_back(_e->getStart());
        }
        else{
            vertices.push_back(_e->getStart());
            f = jarvis.calculate(_obj, vertices, _e->getEnd()->getPoint() - _e->getStart()->getPoint(), _e->getEnd());

            vertices.push_back(_e->getEnd());
        }

        f->setVerticesInThePlan(vertices);
        return f;
    }

    Vertex *v3 = vertices.front();
    v3->setIsBoundary(true);

    Face *f = new Face(faceID++, _obj, _e, !_es, true);
    _obj->addFace(f);

    vertices.push_back(_e->getStart());
    vertices.push_back(_e->getEnd());
    f->setVerticesInThePlan(vertices);

    Edge *e2, *e3;
    bool e2Signal;
    bool e3Signal;
    if(_es){//se a nova face for a counter clockwise de e
        e2 = new Edge(edgeID++, _e->getStart(), v3, true);
        e2Signal = _obj->addEdge(&e2);
        if(e2->getID() != edgeID - 1) edgeID = edgeID - 1;

        e3 = new Edge(edgeID++, v3, _e->getEnd(), true);
        e3Signal = _obj->addEdge(&e3);
        if(e3->getID() != edgeID - 1) edgeID = edgeID - 1;

        _e->setCounterClockwise(f);
        _e->setNextCounterClockwise(e2, e2Signal);
        _e->setPreviousCounterClockwise(e3, e3Signal);
    }else{//caso seja clockwise
        e2 = new Edge(edgeID++, _e->getEnd(), v3, true);
        e2Signal = _obj->addEdge(&e2);
        if(e2->getID() != edgeID - 1) edgeID = edgeID - 1;

        e3 = new Edge(edgeID++, v3, _e->getStart(), true);
        e3Signal = _obj->addEdge(&e3);
        if(e3->getID() != edgeID - 1) edgeID = edgeID - 1;

        _e->setClockwise(f);
        _e->setNextClockwise(e2, e2Signal);
        _e->setPreviousClockwise(e3, e3Signal);
    }


    v3->setEdge(e3, e3Signal);

    if(e2Signal){
        e2->setClockwise(f);
        e2->setNextClockwise(e3, e3Signal);
        e2->setPreviousClockwise(_e, !_es);
    }
    else{
        e2->setCounterClockwise(f);
        e2->setNextCounterClockwise(e3, e3Signal);
        e2->setPreviousCounterClockwise(_e, !_es);
    }

    if(e3Signal){
        e3->setClockwise(f);
        e3->setNextClockwise(_e, !_es);
        e3->setPreviousClockwise(e2, e2Signal);
    }
    else{
        e3->setCounterClockwise(f);
        e3->setNextCounterClockwise(_e, !_es);
        e3->setPreviousCounterClockwise(e2, e2Signal);
    }


    return f;
}

std::vector<Vertex *>GiftWrapping::findNextVertex(Object *_obj, Face *_f, Edge *_e, bool _es){
    std::vector<Vertex *> originalVertices = _obj->getVertices();


    double maxCosAlpha = -2; //com isso teremos o menor angulo alfa e o maior beta como queriamos
    double currentCosAlpha;
    std::vector<Vertex *> vertices;
    std::vector<Vertex *> verticesParallel;

    Point nf = _f->normal();
    Point nfi;

    Vertex vi, *v, vEStart, vEEnd;
    Face fi;
    int localEdgeID = edgeID;

    v = _e->getStart();
    vEStart = Vertex(-v->getID(), v->getPoint());
    v = _e->getEnd();
    vEEnd = Vertex(-v->getID(), v->getPoint());

    Edge e = Edge(-(localEdgeID++), &vEStart, &vEEnd);
    Edge ei1, ei2;

    for(unsigned int i = 0; i < originalVertices.size(); i++){
        //criar a nova face
        v = originalVertices.at(i);
        vi = Vertex(-v->getID(), v->getPoint());


        if(v == _e->getStart() || v == _e->getEnd()) continue;


        fi = Face(faceID, NULL, &e, !_es, true);//colocando true porque se não não se consegue a normal
        ei1 = Edge(-(localEdgeID++), &vi, e.getEnd());
        ei2 = Edge(-(localEdgeID++), e.getStart(), &vi);

        vi.setEdge(&ei1, true);

        ei1.setClockwise(&fi);
        if(_es) e.setCounterClockwise(&fi);
        else e.setClockwise(&fi);
        ei2.setClockwise(&fi);

        ei1.setNextClockwise(&e, !_es);
        if(_es) e.setNextCounterClockwise(&ei2, true);
        else e.setNextClockwise(&ei2, true);
        ei2.setNextClockwise(&ei1, true);

        ei1.setPreviousClockwise(&ei2, true);
        if(_es) e.setPreviousCounterClockwise(&ei1, true);
        else e.setPreviousClockwise(&ei1, true);
        ei2.setPreviousClockwise(&e, !_es);

        //calcular o angulo
        if(isParallel(e, ei1)){
            verticesParallel.push_back(v);
            continue;
        }else{
            nfi = fi.normal();
        }
        currentCosAlpha = cosAlpha(nf, nfi);

        //comparar os angulos.... se achar um maior apaga o vetor e adiciona, se achar um igual somente adiciona
        if ((currentCosAlpha >= maxCosAlpha - ERROR) && (currentCosAlpha <= maxCosAlpha + ERROR)){//igual
            vertices.push_back(v);
        }else if(currentCosAlpha > maxCosAlpha){
            maxCosAlpha = currentCosAlpha;
            vertices.clear();
            vertices.push_back(v);
        }
    }

    vertices.insert(vertices.end(), verticesParallel.begin(), verticesParallel.end());
    return vertices;
}

Point GiftWrapping::firstAxis(std::vector<Vertex *> _vertices, Vertex *_v1, std::vector<Vertex *> _originalVertices){

    Point normal, p1 = _v1->getPoint();
    bool toBreak = false;

    //primeiro a se fazer, a char a verdadeira normal da face.
    for(unsigned int i = 0; i < _vertices.size(); i++){
        for(unsigned int j = i + 1; j < _vertices.size(); j++){
            normal = ((_vertices.at(i)->getPoint() - p1).normalization()).crossProduct((_vertices.at(j)->getPoint() - p1).normalization());
            if(normal.length() == 1.0){

                continue;
            }
            else{
                normal = normal.normalization();

                toBreak = true;
                break;
            }
        }
        if(toBreak) break;
    }


    Point pi;
    bool inPlan;
    //testando se a normal está do lado certo.
    for(unsigned int i = 0; i < _originalVertices.size(); i++){
        inPlan = false;
        if(_originalVertices.at(i) == _v1) continue;
        for(unsigned int j = 0; j < _vertices.size(); j++){
            if(_originalVertices.at(i) == _vertices.at(j)){
                inPlan = true;
                break;
            }
        }
        if(!inPlan){
            pi = _originalVertices.at(i)->getPoint() - p1;
            if(cosAlpha(pi.normalization(), normal) >= 0.0 ){//***pode da erro de precisão
                normal = normal*(-1.0);
                normal.print();
            }
            break;
        }
    }

    Point parcialAxis = normal.crossProduct(p1 - _vertices.front()->getPoint());


    int max = -1;
    double maxCosAlpha = -2, currentCosAlpha;
    double testNormal;
    Point currentVector;
    for(unsigned int i = 0; i < _vertices.size(); i++){
        currentVector = _vertices.at(i)->getPoint() - p1;
        testNormal = ((currentVector).crossProduct(parcialAxis).normalization()).dotProduct(normal);
        if(!((testNormal + ERROR >= -1.0) && (testNormal - ERROR <= 1.0))){//igual a -1 - oposto

            currentCosAlpha = cosAlpha(parcialAxis, currentVector);

            //comparar os angulos.... se achar um maior apaga o vetor e adiciona, se achar um igual somente adiciona
            if(currentCosAlpha > maxCosAlpha){
                maxCosAlpha = currentCosAlpha;
                max = i;
            }
        }
    }
    if(max == -1){
        return parcialAxis;
    }
    return _vertices.at(max)->getPoint() - p1;
}
